//	--------------------------------------------------------------------
//	Copyright(C) 2006,2007 Zhao Yukun. All rights reserved.
//	This file is a part of the Xeres system.
//	Xeres 2007
//	Contact : xeres.engine@gmail.com
//	$(reservedInfo)
//	$(reservedDeclaration)
//	--------------------------------------------------------------------
///	@file	<math_ext.inl>
///	@path	~/src/lib/math/
///	@date	2007/09/08
///	@desc	mathematics extension.

#pragma once

#include "lib/math/math_ext.h"

namespace xeres {

	/* fast sin cos */
	static inline void sincos( const float radians , float * sine , float * cosine )
	{
#if (!defined _WIN64) || (defined __ICL)
		_asm
		{
			fld			DWORD PTR [radians];
			fsincos		;
			mov			edx , cosine;
			mov			eax , sine;
			fstp		DWORD PTR [edx];
			fstp		DWORD PTR [eax];
		}

#else // (!defined _WIN64) || (defined __ICL)
		*sine = sinf( radians );
		*cosine = cosf( radians );
#endif // (!defined _WIN64) || (defined __ICL)
	}
	static inline void sincos( const double radians , double * sine , double * cosine )
	{
#if (!defined _WIN64) || (defined __ICL)
		_asm
		{
			fld			QWORD PTR [radians];
			fsincos		;
			mov			edx , cosine;
			mov			eax , sine;
			fstp		QWORD PTR [edx];
			fstp		QWORD PTR [eax];
		}
#else // (!defined _WIN64) || (defined __ICL)
		*sine = sin( radians );
		*cosine = cos( radians );
#endif // (!defined _WIN64) || (defined __ICL)
	}

	// spline basis
	struct SplineBasis
	{
		enum
		{
			Bezier  = 0,
			BSpline ,
			CatmullRom ,
			Hermite ,
			Power ,
		};
	};

	static const float SplineBasisValueF[5][16] = 
	{
		{// bezier
			-1,  3, -3,  1,
			 3, -6,  3,  0,
			-3,  3,  0,  0,
			 1,  0,  0,  0
		},
		{// bspline
			-1/6.f,  3/6.f, -3/6.f,  1/6.f,
			 3/6.f, -6/6.f,  3/6.f,  0,
			-3/6.f,      0,  3/6.f,  0,
			 1/6.f,  4/6.f,  1/6.f,  0
		},
		{// catmullrom
			-1/2.f,  3/2.f, -3/2.f,  1/2.f,
			 2/2.f, -5/2.f,  4/2.f, -1/2.f,
			-1/2.f,      0,  1/2.f,      0,
			     0,  2/2.f,      0,      0
		},
		{// hermite
			 2,  1, -2,  1,
			-3, -2,  3, -1,
			 0,  1,  0,  0,
			 1,  0,  0,  0
		},
		{// power
			1,  0,  0,  0,
			0,  1,  0,  0,
			0,  0,  1,  0,
			0,  0,  0,  1
		},
	};

	/* spline */
	template< typename _Ty > inline _Ty spline( const _Ty basis[16] , const _Ty value , const _Ty knot[4] ) {
		_Ty c3 = _Ty((basis[ 0]*knot[0]) + (_Ty)(basis[ 1]*knot[1]) + (_Ty)(basis[ 2]*knot[2]) + (_Ty)(basis[ 3]*knot[3]));
		_Ty c2 = _Ty((basis[ 4]*knot[0]) + (_Ty)(basis[ 5]*knot[1]) + (_Ty)(basis[ 6]*knot[2]) + (_Ty)(basis[ 7]*knot[3]));
		_Ty c1 = _Ty((basis[ 9]*knot[0]) + (_Ty)(basis[ 9]*knot[1]) + (_Ty)(basis[10]*knot[2]) + (_Ty)(basis[11]*knot[3]));
		_Ty c0 = _Ty((basis[12]*knot[0]) + (_Ty)(basis[13]*knot[1]) + (_Ty)(basis[14]*knot[2]) + (_Ty)(basis[15]*knot[3]));
		_Ty t2 = value * value;
		_Ty t3 = t2 * value;
		return c3 * t3 + c2 * t2 + c1 * value + c0;
	}

	/* polynomial solving */

#define	cbrt(x)		((x) > 0.0 ? pow((T)(x), 1.0/3.0) : \
					((x) < 0.0 ? -pow((T)-(x), 1.0/3.0) : 0.0))


	template <class _Ty> inline int solveLinear( const _Ty& a , const _Ty& b , _Ty &x )
	{
		if( !is_zero( a ) )
		{
			x = -b / a;
			return 1;
		}
		return 0;
	}
	template <typename _Ty> inline int solveQuadric( const _Ty& a , const _Ty& b , const _Ty& c , _Ty r[2] )
	{
		_Ty inva = recip(a);
		_Ty nc = c*inva;
		_Ty nb = b*inva*((_Ty)0.5);
		_Ty delta = nb*nb - nc;
		if( delta < 0 )
			return 0;
		else
		{
			_Ty sqrtDelta = (_Ty)sqrt(delta);
			r[0] = -sqrtDelta - nb;
			r[1] = sqrtDelta - nb;
			return 2;
		}
	}
	template <typename _Ty> inline int solveCubic( const _Ty c[4] , _Ty s[3] )
	{
		int i, num;
		_Ty sub;
		_Ty A, B, C;
		_Ty sq_A, p, q;
		_Ty cb_p, D;
		A = c[2] / c[3];
		B = c[1] / c[3];
		C = c[0] / c[3];
		sq_A	= A * A;
		p = (_Ty)( 1.0 / 3.0 * ( -1.0 / 3.0 * sq_A + B) );
		q = (_Ty)( 1.0 / 2.0 * ( 2.0 / 27.0 * A * sq_A - 1.0 / 3.0 * A * B + C) );

		cb_p	= p * p * p;
		D = q * q + cb_p;

		if( is_zero(D) )
		{
			if( is_zero(q) )
			{
				s[0] = 0;
				num = 1;
			}
			else 
			{
				_Ty u = (_Ty)( cbrt(-q) );
				s[0] = 2 * u;
				s[1] = -u;
				num = 2;
			}
		}
		else if( D < 0 )
		{
			_Ty phi = (_Ty)(1.0/3 * acos(-q / sqrt(-cb_p)));
			_Ty t = (_Ty)(2.0 * sqrt(-p));
			s[0] = t * cos(phi);
			s[1] = -t * cos(phi + ArithmeticOp<_Ty>::pi()/ 3.0);
			s[2] = -t * cos(phi - ArithmeticOp<_Ty>::pi()/ 3.0);
			num = 3;
		}
		else
		{
			_Ty sqrt_D = (_Ty)(sqrt(D));
			_Ty u = (_Ty)(cbrt(sqrt_D - q));
			_Ty v = (_Ty)(-cbrt(sqrt_D + q));
			s[ 0 ] = u + v;
			num = 1;
		}
		sub = (_Ty) (1.0/3.0 * A);
		for (i = 0; i < num; ++i)
			s[ i ] -= sub;
		return num;
	}
	template <typename _Ty> inline int solveQuartic( const _Ty c[5] , _Ty s[4] )
	{
		int i, num;
		_Ty coeffs[4];
		_Ty z, u, v, sub;
		_Ty A, B, C, D;
		_Ty sq_A, p, q, r;
		A = c[3] / c[4];
		B = c[2] / c[4];
		C = c[1] / c[4];
		D = c[0] / c[4];
		sq_A	= A * A;
		p = (_Ty)(- 3.0/8.0 * sq_A + B);
		q = (_Ty)(1.0/8.0 * sq_A * A - 1.0/2.0 * A * B + C);
		r = (_Ty)(-3.0/256.0*sq_A*sq_A + 1.0/16.0*sq_A*B - 1.0/4.0*A*C + D);
		if( is_zero(r) )
		{
			coeffs[0] = q;
			coeffs[1] = p;
			coeffs[2] = 0;
			coeffs[3] = 1;
			num = solveCubic<_Ty>(coeffs, s);
			s[ num++ ] = 0;
		}
		else
		{
			coeffs[0] = 1.0/2.0 * r * p - 1.0/8.0 * q * q;
			coeffs[1] = - r;
			coeffs[2] = - 1.0/2.0 * p;
			coeffs[3] = 1;
			(void) solveCubic<_Ty>(coeffs, s);
			z = s[ 0 ];
			u = z * z - r;
			v = 2 * z - p;
			if( is_zero(u) )
				u = 0;
			else if(u > 0)
				u = sqrt(u);
			else
				return 0;

			if( is_zero(v) )
				v = 0;
			else if( v > 0 )
				v = sqrt(v);
			else
				return 0;
			coeffs[0] = z - u;
			coeffs[1] = q < 0 ? -v : v;
			coeffs[2] = 1;
			num = solveQuadric<_Ty>(coeffs[2],coeffs[1],coeffs[0],s);
			coeffs[0]= z + u;
			coeffs[1] = q < 0 ? v : -v;
			coeffs[2] = 1;
			num += solveQuadric<_Ty>(coeffs[2],coeffs[1],coeffs[0],s + num);
		}
		sub = 1.0/4.0 * A;
		for (i = 0; i < num; ++i)
			s[ i ] -= sub;
		return num;
	}

#undef cbrt

	template< typename _Scalar > inline void adjointx( _Scalar *out,const _Scalar *in )
	{

#define	element(row,column)	(row+(column<<2))

		_Scalar a1, a2, a3, a4, b1, b2, b3, b4;
		_Scalar c1, c2, c3, c4, d1, d2, d3, d4;

		a1 = in[element(0,0)]; b1 = in[element(0,1)]; 
		c1 = in[element(0,2)]; d1 = in[element(0,3)];

		a2 = in[element(1,0)]; b2 = in[element(1,1)]; 
		c2 = in[element(1,2)]; d2 = in[element(1,3)];

		a3 = in[element(2,0)]; b3 = in[element(2,1)];
		c3 = in[element(2,2)]; d3 = in[element(2,3)];

		a4 = in[element(3,0)]; b4 = in[element(3,1)]; 
		c4 = in[element(3,2)]; d4 = in[element(3,3)];

		out[element(0,0)]  = (_Scalar)  det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4);
		out[element(1,0)]  = (_Scalar) -det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4);
		out[element(2,0)]  = (_Scalar)  det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4);
		out[element(3,0)]  = (_Scalar) -det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);

		out[element(0,1)]  = (_Scalar) -det3x3( b1, b3, b4, c1, c3, c4, d1, d3, d4);
		out[element(1,1)]  = (_Scalar)  det3x3( a1, a3, a4, c1, c3, c4, d1, d3, d4);
		out[element(2,1)]  = (_Scalar) -det3x3( a1, a3, a4, b1, b3, b4, d1, d3, d4);
		out[element(3,1)]  = (_Scalar)  det3x3( a1, a3, a4, b1, b3, b4, c1, c3, c4);

		out[element(0,2)]  = (_Scalar)  det3x3( b1, b2, b4, c1, c2, c4, d1, d2, d4);
		out[element(1,2)]  = (_Scalar) -det3x3( a1, a2, a4, c1, c2, c4, d1, d2, d4);
		out[element(2,2)]  = (_Scalar)  det3x3( a1, a2, a4, b1, b2, b4, d1, d2, d4);
		out[element(3,2)]  = (_Scalar) -det3x3( a1, a2, a4, b1, b2, b4, c1, c2, c4);

		out[element(0,3)]  = (_Scalar) -det3x3( b1, b2, b3, c1, c2, c3, d1, d2, d3);
		out[element(1,3)]  = (_Scalar)  det3x3( a1, a2, a3, c1, c2, c3, d1, d2, d3);
		out[element(2,3)]  = (_Scalar) -det3x3( a1, a2, a3, b1, b2, b3, d1, d2, d3);
		out[element(3,3)]  = (_Scalar)  det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3);

#undef element

	}

	/* SSE optimized intrinsic */

	/* a fix of cvtsd_f64 & cvtss_f32 */
#ifndef	__ICL
	inline double _mm_cvtsd_f64( __m128d a )
	{
		double ret;
		_mm_store_sd( &ret , a );
		return ret;
	}
	inline float _mm_cvtss_f32( __m128 a )
	{
		float ret;
		_mm_store_ss( &ret , a );
		return ret;
	}
#endif
	/* component access */
	template< int comp > inline __m128 _mm_comp_pd( __m128d a )
	{
		assert( comp >= 0 && comp < 2 );
		return _mm_shuffle_pd( a , a , _MM_SHUFFLE2( comp , comp ) );
	}
	template< int comp > inline __m128 _mm_comp_ps( __m128 a )
	{
		assert( comp >= 0 && comp < 4 );
		return _mm_shuffle_ps( a , a , _MM_SHUFFLE( comp , comp , comp , comp ) );
	}
	inline __m128d _mm_sum_pd( __m128d x ) {
		__m128d t = _mm_shuffle_pd( x , x , _MM_SHUFFLE2( 1 , 1 ) );
		return _mm_add_sd( t , x );
	}
	/* x[0] + x[1] + x[2] */
	inline __m128 _mm_sum_ps3( __m128 x ) {
		__m128 t = _mm_add_ss( x , _mm_movehl_ps( x , x ) );
		t = _mm_add_ss( t , _mm_shuffle_ps( x , x , 1 ) );
		return t;
	}
	/* x[0] + x[1] + x[2] + x[3] */
	inline __m128 _mm_sum_ps4( __m128 x ) {
		__m128 r;
		r = _mm_add_ps( x , _mm_movehl_ps( x , x ) );
		r = _mm_add_ss( r , _mm_shuffle_ps( r , r , 1 ) );
		return r;
	}
	/* x[0] * x[0] + y[0] * y[0] */
	inline __m128d _mm_square_pd( __m128d x ) {
		__m128d s;
		s = _mm_mul_pd( x , x );
		s = _mm_add_sd( s , _mm_shuffle_pd( s , s , 1 ) );
		return s;
	}
	/* x[0] * x[0] + y[0] * y[0] + z[0] * z[0] */
	inline __m128 _mm_square_ps3( __m128 x ) {
		__m128 s , r;
		s = _mm_mul_ps( x , x );
		r = _mm_add_ss( s , _mm_movehl_ps( s , s ) );
		r = _mm_add_ss( r , _mm_shuffle_ps( r , r , 1 ) );
		return r;
	}
	/* x[0] * x[0] + y[0] * y[0] + z[0] * z[0] + w[0] * w[0] */
	inline __m128 _mm_square_ps4( __m128 x ) {
		__m128 s , r;
		s = _mm_mul_ps( x , x );
		r = _mm_add_ps( s , _mm_movehl_ps( s , s ) );
		r = _mm_add_ss( r , _mm_shuffle_ps( r , r , 1 ) );
		return r;
	}

	/* x[0] * x[1] + y[0] * y[1] */
	inline __m128d _mm_dot_pd( __m128d x , __m128d y ) {
		__m128d s;
		s = _mm_mul_pd( x , y );
		s = _mm_add_sd( s , _mm_shuffle_pd( s , s , 1 ) );
		return s;
	}
	/* x[0] * x[1] + y[0] * y[1] + z[0] * z[1] */
	inline __m128 _mm_dot_ps3( __m128 x , __m128 y ) {
		__m128 s , r;
		s = _mm_mul_ps( x , y );
		r = _mm_add_ss( s , _mm_movehl_ps( s , s ) );
		r = _mm_add_ss( r , _mm_shuffle_ps( r , r , 1 ) );
		return r;
	}
	/* x[0] * x[1] + y[0] * y[1] + z[0] * z[1] + w[0] * w[1] */
	inline __m128 _mm_dot_ps4( __m128 x , __m128 y ) {
		__m128 s , r;
		s = _mm_mul_ps( x , y );
		r = _mm_add_ps( s , _mm_movehl_ps( s , s ) );
		r = _mm_add_ss( r , _mm_shuffle_ps( r , r , 1 ) );
		return r;
	}
	/* cross */
	inline __m128 _mm_cross_ps3( __m128 a , __m128 b ) {
		__m128 ea , eb;
		// set to a[1][2][0][3] , b[2][0][1][3]
		ea = _mm_shuffle_ps( a , a , _MM_SHUFFLE( 3 , 0 , 2 , 1 ) );
		eb = _mm_shuffle_ps( b , b , _MM_SHUFFLE( 3 , 1 , 0 , 2 ) );
		// multiply
		__m128 xa = _mm_mul_ps( ea , eb );
		// set to a[2][0][1][3] , b[1][2][0][3]
		a = _mm_shuffle_ps( a , a , _MM_SHUFFLE( 3 , 1 , 0 , 2 ) );
		b = _mm_shuffle_ps( b , b , _MM_SHUFFLE( 3 , 0 , 2 , 1 ) );
		// multiply
		__m128 xb = _mm_mul_ps( a , b );
		// subtract
		return _mm_sub_ps( xa , xb );
	}
	/* r = 1 / a */
	/* [2 * rcpps(x) - (x * rcpps(x) * rcpps(x))] */
	inline __m128 _mm_rcpnr_ps( __m128 a )
	{
		__m128 rcp = _mm_rcp_ps( a );
		/* approximate closer */
		rcp = _mm_sub_ps( _mm_add_ps( rcp , rcp ) , _mm_mul_ps( a , _mm_mul_ps( rcp , rcp ) ) );
		return rcp;
	}
	inline __m128 _mm_rcpnr_ss( __m128 a )
	{
		/* divisor */
		/* reciple */
		__m128 rcp = _mm_rcp_ss( a );
		/* approximate closer */
		rcp = _mm_sub_ss( _mm_add_ss( rcp , rcp ) , _mm_mul_ss( a , _mm_mul_ss( rcp , rcp ) ) );
		return rcp;
	}
	/* r = 1 / sqrt(a) */
	/* 0.5 * rsqrtss * (3 - x * rsqrtss(x) * rsqrtss(x)) */
	inline __m128 _mm_rsqrtnr_ps( __m128 a )
	{
		/* divisor */
		static const __m128 _05 = _mm_set1_ps( 0.5f );
		static const __m128 _3 = _mm_set1_ps( 3.f );
		/* recipe square root */
		__m128 rsqrt = _mm_rsqrt_ps( a );
		/* approximate closer */
		rsqrt = _mm_mul_ps( _mm_mul_ps( _05 , rsqrt ) , _mm_sub_ps( _3 , _mm_mul_ps( a , _mm_mul_ps( rsqrt , rsqrt ) ) ) );
		return rsqrt;
	}
	inline __m128 _mm_rsqrtnr_ss( __m128 a )
	{
		/* divisor */
		static const __m128 _05 = _mm_set1_ps( 0.5f );
		static const __m128 _3 = _mm_set1_ps( 3.f );
		/* recipe square root */
		__m128 rsqrt = _mm_rsqrt_ss( a );
		/* approximate closer */
		rsqrt = _mm_mul_ss( _mm_mul_ss( _05 , rsqrt ) , _mm_sub_ss( _3 , _mm_mul_ss( a , _mm_mul_ss( rsqrt , rsqrt ) ) ) );
		return rsqrt;
	}
	/* abs */
	inline __m128 _mm_abs_ps( __m128 a )
	{
		static const union { int i[4]; __m128 m; } __mm_abs_mask_cheat_ps = {0x7fffffff, 0x7fffffff, 0x7fffffff, 0x7fffffff};
		return _mm_and_ps( a, __mm_abs_mask_cheat_ps.m );
	}
	inline __m128d _mm_abs_pd( __m128d a )
	{
		static const union { int i[4]; __m128d m; } __mm_abs_mask_cheat_pd = {0xffffffff, 0x7fffffff, 0xffffffff, 0x7fffffff};
		return _mm_and_pd( a, __mm_abs_mask_cheat_pd.m );
	}
	/* negative */
	inline __m128 _mm_neg_ps( __m128 a )
	{
		static const union { int i[4]; __m128 m; } __mm_inv_mask_cheat_ps = {0x80000000, 0x80000000, 0x80000000, 0x80000000};
		return _mm_xor_ps( a, __mm_inv_mask_cheat_ps.m );
	}
	inline __m128d _mm_neg_pd( __m128d a )
	{
		static const union { int i[4]; __m128d m; } __mm_inv_mask_cheat_pd = {0x00000000, 0x80000000, 0x00000000, 0x80000000};
		return _mm_xor_pd( a, __mm_inv_mask_cheat_pd.m );
	}
	/* comparison with epsilon */
	inline bool _mm_equal_ps4( __m128 a , __m128 b )
	{
		static const __m128 xmm_epsilon = _mm_set1_ps( ArithmeticOp<float>::comparison_epsilon() );
		__m128 cmp = _mm_sub_ps( _mm_max_ps( a , b ) , _mm_min_ps( a , b ) );
		cmp = _mm_abs_ps( cmp );
		return _mm_movemask_ps( _mm_cmpge_ps( cmp , xmm_epsilon ) ) == 0;
	}
	inline bool _mm_equal_ps3( __m128 a , __m128 b )
	{
		static const int mask = 1<<2 | 1<<1 | 1<<0;
		const __m128 xmm_epsilon = _mm_set1_ps( ArithmeticOp<float>::comparison_epsilon() );
		__m128 cmp = _mm_sub_ps( _mm_max_ps( a , b ) , _mm_min_ps( a , b ) );
		cmp = _mm_abs_ps( cmp );
		return ( _mm_movemask_ps( _mm_cmpge_ps( cmp , xmm_epsilon ) ) & mask ) == 0;
	}
	inline bool _mm_equal_pd( __m128d a , __m128d b )
	{
		static const __m128d xmm_epsilon = _mm_set1_pd( ArithmeticOp<double>::comparison_epsilon() );
		__m128d cmp = _mm_sub_pd( _mm_max_pd( a , b ) , _mm_min_pd( a , b ) );
		cmp = _mm_abs_pd( cmp );
		return _mm_movemask_pd( _mm_cmpge_pd( cmp , xmm_epsilon ) ) == 0;
	}
	/* reflect */
	inline __m128 _mm_refl_ps( __m128 n , __m128 i )
	{
		__m128 i_dot_n = _mm_dot_ps3( n , i );
		i_dot_n = _mm_add_ss( i_dot_n , i_dot_n );
		i_dot_n = _mm_shuffle_ps( i_dot_n , i_dot_n , 0 );
		return _mm_sub_ps( i , _mm_mul_ps( n , i_dot_n ) );
	}
	/* refract */
	inline __m128 _mm_refr_ps( __m128 n , __m128 i , __m128 eta )
	{
		static const __m128 _0 = _mm_setzero_ps();
		static const __m128 _1 = _mm_set1_ps( 1.f );
		__m128 i_dot_n = _mm_dot_ps3( n , i );
		__m128 k = _mm_sub_ss( _1 , _mm_mul_ss( i_dot_n , i_dot_n ) );
		k = _mm_sub_ss( _1 , _mm_mul_ss( _mm_mul_ss( eta , eta ) , k ) );
		/* comparison */
		if( _mm_comilt_ss( k , _0 ) )
		{
			return _0;
		}
		else
		{
			__m128 m1 = _mm_mul_ps( i , eta );
			__m128 m2 = _mm_add_ss( _mm_sqrt_ss( k ) , _mm_mul_ss( eta , i_dot_n ) );
			return _mm_sub_ps( m1 , _mm_mul_ps( n , _mm_shuffle_ps( m2 , m2 , 0 ) ) );
		}
	}
	inline __m128 _mm_normalize_ps( __m128 a )
	{
		__m128 l = _mm_square_ps3( a );
		l = _mm_rsqrtnr_ss( l );
		l = _mm_shuffle_ps( l , l , 0 );
		return _mm_mul_ps( a , l );
	}
	/* clamp value */
	inline __m128 _mm_locut_ps( __m128 val , __m128 bound )
	{
		return _mm_max_ps( val , bound );
	}
	inline __m128 _mm_hicut_ps( __m128 val , __m128 bound )
	{
		return _mm_min_ps( val , bound );
	}
	inline __m128 _mm_clamp_ps( __m128 val , __m128 min , __m128 max )
	{
		return _mm_locut_ps( _mm_hicut_ps( val , max ) , min );
	}
	/* pack vector */
	inline __m128 _mm_pack_ps3( __m128 a , __m128 b , __m128 c )
	{
		return  _mm_movelh_ps( _mm_unpacklo_ps( a , b ) , c );
	}
	inline __m128 _mm_pack_ps3( float a , float b , float c )
	{
		return  _mm_movelh_ps( _mm_unpacklo_ps(
				_mm_load_ss( &a ) ,
				_mm_load_ss( &b ) ) ,
				_mm_load_ss( &c ) );
	}
	inline __m128 _mm_pack_ps4( const __m128& a , const __m128& b , const __m128& c , const __m128& d )
	{
		return  _mm_movelh_ps(
			_mm_unpacklo_ps( a , b ) ,
			_mm_unpacklo_ps( c , d ) );
	}
	inline __m128 _mm_pack_ps4( float a , float b , float c , float d )
	{
		return  _mm_movelh_ps(
			_mm_unpacklo_ps(
				_mm_load_ss( &a ) , _mm_load_ss( &b ) ) ,
			_mm_unpacklo_ps(
				_mm_load_ss( &c ) , _mm_load_ss( &d ) ) );
	}
	/* select op */
#define MM_SELECT(op) \
	inline __m128 _mm_select_##op##_ps( const __m128& a , const __m128& b , const __m128& c , const __m128& d ) \
	{																\
		__m128 mask = _mm_cmp##op##_ps( a , b );					\
		return( _mm_or_ps( _mm_and_ps( mask , c ) , _mm_andnot_ps( mask , d ) ) ); \
	}
MM_SELECT(eq)
MM_SELECT(lt)
MM_SELECT(le)
MM_SELECT(gt)
MM_SELECT(ge)
MM_SELECT(neq)
MM_SELECT(nlt)
MM_SELECT(nle)
MM_SELECT(ngt)
MM_SELECT(nge)
#undef MM_SELECT

	/* comparison boolean */
#define MM_BCMP(op) \
	inline __m128i _mm_bcmp_##op##_ps( __m128 a , __m128 b ) \
	{																\
		static const union { float f[4]; __m128 m; } __mm_one_ps = { 1 , 1 , 1 , 1 };	\
		static const union { float f[4]; __m128 m; } __mm_zero_ps = { 0 , 0 , 0 , 0 };	\
		__m128 mask = _mm_cmp##op##_ps( a , b );					\
		return( _mm_cvttps_epi32( _mm_or_ps( _mm_and_ps( mask , __mm_one_ps.m ) , _mm_andnot_ps( mask , __mm_zero_ps.m ) ) ) ); \
	}
MM_BCMP(eq)
MM_BCMP(lt)
MM_BCMP(le)
MM_BCMP(gt)
MM_BCMP(ge)
MM_BCMP(neq)
MM_BCMP(nlt)
MM_BCMP(nle)
MM_BCMP(ngt)
MM_BCMP(nge)
#undef MM_BCMP
#define MM_SELECT(op) \
	inline __m128d _mm_select_##op##_pd( const __m128d& a , const __m128d& b , const __m128d& c , const __m128d& d ) \
	{																\
		__m128d mask = _mm_cmp##op##_pd( a , b );					\
		return( _mm_or_pd( _mm_and_pd( mask , c ) , _mm_andnot_pd( mask , d ) ) ); \
	}
MM_SELECT(eq)
MM_SELECT(lt)
MM_SELECT(le)
MM_SELECT(gt)
MM_SELECT(ge)
MM_SELECT(neq)
MM_SELECT(nlt)
MM_SELECT(nle)
MM_SELECT(ngt)
MM_SELECT(nge)
#undef MM_SELECT

	/* comparison boolean */
#define MM_BCMP(op) \
	inline __m128i _mm_bcmp_##op##_pd( __m128d a , __m128d b ) \
	{																\
		static const union { double f[2]; __m128d m; } __mm_one_ps = { 1 , 1 };	\
		static const union { double f[2]; __m128d m; } __mm_zero_ps = { 0 , 0 };	\
		__m128d mask = _mm_cmp##op##_pd( a , b );					\
		return( _mm_cvttpd_epi32( _mm_or_pd( _mm_and_pd( mask , __mm_one_ps.m ) , _mm_andnot_pd( mask , __mm_zero_ps.m ) ) ) ); \
	}
MM_BCMP(eq)
MM_BCMP(lt)
MM_BCMP(le)
MM_BCMP(gt)
MM_BCMP(ge)
MM_BCMP(neq)
MM_BCMP(nlt)
MM_BCMP(nle)
MM_BCMP(ngt)
MM_BCMP(nge)
#undef MM_BCMP
	/* sign op */
	inline __m128 _mm_sign_ps( __m128 a )
	{
		static const union { float f[4]; __m128 m; } __mm_one_ps = { 1 , 1 , 1 , 1 };
		static const union { float f[4]; __m128 m; } __mm_neg_one_ps = { -1 , -1 , -1 , -1 };
		__m128 mask = _mm_select_lt_ps( a , _mm_setzero_ps() , __mm_neg_one_ps.m , a );
		return _mm_select_gt_ps( mask , _mm_setzero_ps() , __mm_one_ps.m , mask );
	}
	/* fast reciprocal */
	template<>inline float recip<float>( const float& a )
	{
		register float r;
		__m128 rcp = _mm_load_ss( &a );
		rcp = _mm_rcp_ss( rcp );
		_mm_store_ss( &r , rcp );
		/* [2 * rcpps(x) - (x * rcpps(x) * rcpps(x))] */
		r = 2.0f * r - ( a * r * r );
		return r;
	}
	template<>inline double recip<double>( const double& a )
	{
		register double r;
		float s = (float)a;
		__m128 rcp = _mm_load_ss( &s );
		rcp = _mm_rcp_ss( rcp );
		_mm_store_ss( &s , rcp );
		r = s;
		/* [2 * rcpps(x) - (x * rcpps(x) * rcpps(x))] */
		r = 2.0 * r - ( a * r * r );
		r = 2.0 * r - ( a * r * r );
		return r;
	}
	/* fast reciprocal square root */
	template<>inline float recip_sqrt<float>( const float& a )
	{
		register float r;
		__m128 rsq = _mm_load_ss( &a );
		rsq = _mm_rsqrt_ss( rsq );
		_mm_store_ss( &r , rsq );
		/* 0.5 * rsqrtss * (3 - x * rsqrtss(x) * rsqrtss(x)) */
		r = 0.5f * r * ( 3.f - a * r * r );
		return r;
	}
	template<>inline double recip_sqrt<double>( const double& a )
	{
		register double r;
		float s = (float)a;
		__m128 rsq = _mm_load_ss( &s );
		rsq = _mm_rsqrt_ss( rsq );
		_mm_store_ss( &s , rsq );
		r = s;
		/* 0.5 * rsqrtss * (3 - x * rsqrtss(x) * rsqrtss(x)) */
		r = 0.5 * r * ( 3.0 - a * r * r );
		r = 0.5 * r * ( 3.0 - a * r * r );
		return r;
	}
	static inline __m128 _mm_floor_ps( __m128 m )
	{
		static const union { float f[4]; __m128 m; } __mm_one_ps = { 1 , 1 , 1 , 1 };
		static const union { float f[4]; __m128 m; } __mm_zero_ps = { 0 , 0 , 0 , 0 };

		__m128i ii = _mm_cvttps_epi32( m );
		__m128 p = _mm_cvtepi32_ps( ii );
		__m128 a = _mm_select_ngt_ps( p , m , __mm_zero_ps.m , __mm_one_ps.m );
		return _mm_sub_ps( p , a );
	}
	static inline __m128 _mm_ceil_ps( __m128 m )
	{
		static const union { float f[4]; __m128 m; } __mm_one_ps = { 1 , 1 , 1 , 1 };
		static const union { float f[4]; __m128 m; } __mm_zero_ps = { 0 , 0 , 0 , 0 };

		__m128i ii = _mm_cvttps_epi32( m );
		__m128 p = _mm_cvtepi32_ps( ii );
		__m128 a = _mm_select_nlt_ps( p , m , __mm_zero_ps.m , __mm_one_ps.m );
		return _mm_add_ps( p , a );
	}
	static inline __m128 _mm_round_ps( __m128 m )
	{
		static const union { float f[4]; __m128 m; } __mm_one_ps = { 1 , 1 , 1 , 1 };
		static const union { float f[4]; __m128 m; } __mm_zero_ps = { 0 , 0 , 0 , 0 };
		static const union { float f[4]; __m128 m; } __mm_05_ps = { 0.5 , 0.5 , 0.5 , 0.5 };

		__m128i ii = _mm_cvttps_epi32( m );
		__m128 p = _mm_cvtepi32_ps( ii );
		__m128 f = _mm_sub_ps( m , p );
		__m128 a = _mm_select_lt_ps( f , __mm_05_ps.m , __mm_zero_ps.m , __mm_one_ps.m );
		return _mm_add_ps( p , a );
	}
	static inline __m128 _mm_radians_ps( __m128 m )
	{
		static const float _pi = ArithmeticOp<float>::pi() / 180.0f;
		static const union { float f[4]; __m128 m; } __mm_rad_ps = { _pi , _pi , _pi , _pi };
		return _mm_mul_ps( m , __mm_rad_ps.m );
	}
	static inline __m128 _mm_degrees_ps( __m128 m )
	{
		static const float _pi = 180.f / ArithmeticOp<float>::pi();
		static const union { float f[4]; __m128 m; } __mm_rad_ps = { _pi , _pi , _pi , _pi };
		return _mm_mul_ps( m , __mm_rad_ps.m );
	}
	static inline __m128 _mm_mod_ps( __m128 m , __m128 f )
	{
		static const union { float f[4]; __m128 m; } __mm_zero_ps = { 0 , 0 , 0 , 0 };
		__m128 p = _mm_div_ps( m , f );
		__m128i ii = _mm_cvttps_epi32( p );
		__m128 pi = _mm_cvtepi32_ps( ii );
		__m128 ss = _mm_mul_ps( f , pi );
		__m128 s = _mm_sub_ps( m , ss );
		__m128 a = _mm_select_lt_ps( s , __mm_zero_ps.m , _mm_abs_ps( f ) , __mm_zero_ps.m );
		return _mm_add_ps( s , a );
	}

} // namespace xeres
